#include "BLDCMotor.h"
#include "Simple_time.h"
#include "math.h"
float velocityOpenloop(BLDCMotor_t *handle,float target_velocity);
float angleOpenloop(BLDCMotor_t *handle,float target_angle);
int alignCurrentSense(BLDCMotor_t *handle);
int alignSensor(BLDCMotor_t *handle);
int absoluteZeroSearch(BLDCMotor_t *handle);
float shaftAngle(BLDCMotor_t *handle);
void setPhaseVoltage(BLDCMotor_t *handle, float Uq, float Ud, float angle_el);
// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
// each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
int trap_120_map[6][3] = {
    {_HIGH_IMPEDANCE,1,-1},
    {-1,1,_HIGH_IMPEDANCE},
    {-1,_HIGH_IMPEDANCE,1},
    {_HIGH_IMPEDANCE,-1,1},
    {1,-1,_HIGH_IMPEDANCE},
    {1,_HIGH_IMPEDANCE,-1} 
};

// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
// each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
int trap_150_map[12][3] = {
    {_HIGH_IMPEDANCE,1,-1},
    {-1,1,-1},
    {-1,1,_HIGH_IMPEDANCE},
    {-1,1,1},
    {-1,_HIGH_IMPEDANCE,1},
    {-1,-1,1},
    {_HIGH_IMPEDANCE,-1,1},
    {1,-1,1},
    {1,-1,_HIGH_IMPEDANCE},
    {1,-1,-1},
    {1,_HIGH_IMPEDANCE,-1},
    {1,1,-1} 
};

// BLDCMotor( int pp , float R)
// - pp            - pole pair number
// - R             - motor phase resistance
// - KV            - motor kv rating (rmp/v)
// - L             - motor phase inductance
void BLDCMotor_Init(BLDCMotor_t *handle,int pp, float _R, float _KV, float _inductance)
{
  // save pole pairs number
  handle->motor_physical.pole_pairs = pp;
  // save phase resistance number
  handle->motor_physical.phase_resistance = _R;
  // save back emf constant KV = 1/KV
  // 1/sqrt(2) - rms value
  handle->motor_physical.KV_rating = _KV*_SQRT2;
  // save phase inductance
  handle->motor_physical.phase_inductance = _inductance;

  // torque control type is voltage by default
  handle->run_config.torque_controller = TorqueControl_voltage;
}


/**
	Link the driver which controls the motor
*/
void linkDriver(BLDCMotor_t *handle,BLDCDriver_t *_driver) 
{
  handle->driver = _driver;
}
void linkSensor(BLDCMotor_t *handle,SimpleSensor_t *_sensor) 
{
  handle->sensor = _sensor;
}
/**
	CurrentSense linking method
*/
void linkCurrentSense( BLDCMotor_t *handle, Simple_CurrentSense_t *_current_sense) 
{
  handle->current_sense = _current_sense;
}

// init hardware pins
void Simple_init(BLDCMotor_t *handle) 
{
  if (!handle->driver || !handle->driver->initialized) 
	{
    handle->motor_status = motor_init_failed;
    //SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
    return;
  }
  handle->motor_status = motor_initializing;
  //SIMPLEFOC_DEBUG("MOT: Init");

  // sanity check for the voltage limit configuration
  if(handle->motor_limit.voltage_limit > handle->driver->voltage_limit) handle->motor_limit.voltage_limit =  handle->driver->voltage_limit;
  // constrain voltage for sensor alignment
  if(handle->run_config.voltage_sensor_align > handle->motor_limit.voltage_limit) handle->run_config.voltage_sensor_align = handle->motor_limit.voltage_limit;

  // update the controller limits
  if( handle->current_sense)
	{
    // current control loop controls voltage
    handle->PID_current_d.limit = handle->motor_limit.voltage_limit;
    handle->PID_current_q.limit = handle->motor_limit.voltage_limit;
  }
  if(_isset(handle->motor_physical.phase_resistance) || handle->run_config.torque_controller != TorqueControl_voltage)
	{
    // velocity control loop controls current
    handle->PID_velocity.limit = handle->motor_limit.current_limit;
  }
	else
	{
    // velocity control loop controls the voltage
    handle->PID_velocity.limit = handle->motor_limit.voltage_limit;
  }
  handle->P_angle.limit = handle->motor_limit.velocity_limit;

  //_delay(500);
  // enable motor
  //SIMPLEFOC_DEBUG("MOT: Enable driver.");
  //enable();
  //_delay(500);
  handle->motor_status = motor_uncalibrated;
}


// disable motor driver
void disable(BLDCMotor_t *handle)
{
    // set zero to PWM
  BLDC_SetPWM(handle->driver,0,0,0);
  // disable the driver
	BLDC_disable(handle->driver);
  // motor status update
  handle->enabled = 0;
}
// enable motor driver
void enable(BLDCMotor_t *handle)
{
  // enable the driver
 BLDC_enable(handle->driver);
 handle->driver->function.enable();
  // set zero to PWM
BLDC_SetPWM(handle->driver,0,0,0);
  // motor status update
 handle->enabled = 1;
}

/**
  FOC functions
*/
// FOC initialization function
int  initFOC(BLDCMotor_t *handle, float zero_electric_offset, Direction _sensor_direction)
{
  int exit_flag = 1;

  handle->motor_status =motor_calibrating;

  // align motor if necessary
  // alignment necessary for encoders!
  if(_isset(zero_electric_offset))
	{
    // abosolute zero offset provided - no need to align
    handle->sensor_config.zero_electric_angle = zero_electric_offset;
    // set the sensor direction - default CW
    handle->sensor_config.sensor_direction = _sensor_direction;
  }

  // sensor and motor alignment - can be skipped
  // by setting motor.sensor_direction and motor.zero_electric_angle
  Simple_Delay(500);
  if(handle->sensor)
	{
    exit_flag *= alignSensor(handle);
    // added the shaft_angle update
    Simple_Sensor_update(handle->sensor);
    handle->run_state.shaft_angle = shaftAngle(handle);
  }
	else 
	{
    exit_flag = 0; // no FOC without sensor
    //SIMPLEFOC_DEBUG("MOT: No sensor.");
  }
  // aligning the current sensor - can be skipped
  // checks if driver phases are the same as current sense phases
  // and checks the direction of measuremnt.
  Simple_Delay(500);
  if(exit_flag)
	{
    if(handle->current_sense)
		{ 
      if (!handle->current_sense->initialized) 
			{
        handle->motor_status = motor_calib_failed;
        //SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized");
        exit_flag = 0;
      }
			else
			{
        exit_flag *= alignCurrentSense(handle);
      }
    }
    //else SIMPLEFOC_DEBUG("MOT: No current sense.");
  }

  if(exit_flag)
	{
    //SIMPLEFOC_DEBUG("MOT: Ready.");
    handle->motor_status = motor_ready;
  }
	else
	{
    //SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
    handle->motor_status = motor_calib_failed;
    disable(handle);
  }

  return exit_flag;
}
// shaft velocity calculation
float shaftVelocity(BLDCMotor_t *handle)
{
  // if no sensor linked return previous value ( for open loop )
  if(!handle->sensor) 
		return handle->run_state.shaft_velocity;
  return handle->sensor_config.sensor_direction*LowPassFilter_GetValue(&handle->LPF_velocity, Simple_Sensor_getVelocity(handle->sensor));
}

float electricalAngle(BLDCMotor_t *handle)
{
  // if no sensor linked return previous value ( for open loop )
  if(!handle->sensor) 
		return handle->run_state.electrical_angle;
	float m_a=Simple_Sensor_getMechanicalAngle(handle->sensor);
	float e_a=(float)(handle->sensor_config.sensor_direction * handle->motor_physical.pole_pairs)*m_a-handle->sensor_config.zero_electric_angle;
  return  _normalizeAngle( e_a);
}

// shaft angle calculation
float shaftAngle(BLDCMotor_t *handle) 
{
  // if no sensor linked return previous value ( for open loop )
  if(!handle->sensor) 
		return handle->run_state.shaft_angle;
  return handle->sensor_config.sensor_direction*LowPassFilter_GetValue(&handle->LPF_angle,Simple_Sensor_getAngle(handle->sensor)) - handle->sensor_config.sensor_offset;
}
// Calibarthe the motor and current sense phases
int alignCurrentSense(BLDCMotor_t *handle)	
{
  int exit_flag = 1; // success

  //SIMPLEFOC_DEBUG("MOT: Align current sense.");

  // align current sense and the driver
  exit_flag = simple_CurrentSense_driverAlign(handle->current_sense,handle->run_config.voltage_sensor_align);
  if(!exit_flag)
	{
    // error in current sense - phase either not measured or bad connection
    //SIMPLEFOC_DEBUG("MOT: Align error!");
    exit_flag = 0;
  }
	else
	{
    // output the alignment status flag
   // SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
  }

  return exit_flag > 0;
}
// Encoder alignment to electrical 0 angle
int alignSensor(BLDCMotor_t *handle) 
{
  int exit_flag = 1; //success
  //SIMPLEFOC_DEBUG("MOT: Align sensor.");

  // check if sensor needs zero search
  if(Simple_Sensor_needsSearch(handle->sensor)) exit_flag = absoluteZeroSearch(handle);
  // stop init if not found index
  if(!exit_flag) return exit_flag;

  // if unknown natural direction
  if(!_isset(handle->sensor_config.sensor_direction)){

    // find natural direction
    // move one electrical revolution forward
    for (int i = 0; i <=500; i++ ) {
      float angle = _3PI_2 + _2PI * i / 500.0f;
      setPhaseVoltage(handle,handle->run_config.voltage_sensor_align, 0,  angle);
	    Simple_Sensor_update(handle->sensor);
      Simple_Delay(2);
    }
    // take and angle in the middle
    Simple_Sensor_update(handle->sensor);
    float mid_angle = Simple_Sensor_getAngle(handle->sensor);
    // move one electrical revolution backwards
    for (int i = 500; i >=0; i-- ) {
      float angle = _3PI_2 + _2PI * i / 500.0f ;
      setPhaseVoltage(handle,handle->run_config.voltage_sensor_align, 0,  angle);
	    Simple_Sensor_update(handle->sensor);;
      Simple_Delay(2);
    }
    Simple_Sensor_update(handle->sensor);
    float end_angle = Simple_Sensor_getAngle(handle->sensor);
    setPhaseVoltage(handle,0, 0, 0);
    Simple_Delay(200);
    // determine the direction the sensor moved
    float moved =  fabs(mid_angle - end_angle);
    if (moved<MIN_ANGLE_DETECT_MOVEMENT) { // minimum angle to detect movement
      //SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
      return 0; // failed calibration
    } else if (mid_angle < end_angle) {
      //SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
      handle->sensor_config.sensor_direction = Direction_CCW;
    } else{
      //SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
      handle->sensor_config.sensor_direction = Direction_CW;
    }
    // check pole pair number
    if( fabs(moved*handle->motor_physical.pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
      //SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
    }
      //SIMPLEFOC_DEBUG("MOT: PP check: OK!");

  } 
  //else SIMPLEFOC_DEBUG("MOT: Skip dir calib.");

  // zero electric angle not known
  if(!_isset(handle->sensor_config.zero_electric_angle)){
    // align the electrical phases of the motor and sensor
    // set angle -90(270 = 3PI/2) degrees
    setPhaseVoltage(handle,handle->run_config.voltage_sensor_align, 0,  _3PI_2);
    Simple_Delay(700);
    // read the sensor
    Simple_Sensor_update(handle->sensor);
    // get the current zero electric angle
    handle->sensor_config.zero_electric_angle = 0;
    handle->sensor_config.zero_electric_angle = electricalAngle(handle);
    //zero_electric_angle =  _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
    Simple_Delay(20);
      //SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
    // stop everything
    setPhaseVoltage(handle,0, 0, 0);
    Simple_Delay(200);
  }
  //else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
  return exit_flag;
}

// Encoder alignment the absolute zero angle
// - to the index
int absoluteZeroSearch(BLDCMotor_t *handle)
{
  // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
  //                    of float is sufficient.
  //SIMPLEFOC_DEBUG("MOT: Index search...");
  // search the absolute zero with small velocity
  float limit_vel = handle->motor_limit.velocity_limit;
  float limit_volt = handle->motor_limit.voltage_limit;
  handle->motor_limit.velocity_limit = handle->run_config.velocity_index_search;
  handle->motor_limit.voltage_limit = handle->run_config.voltage_sensor_align;
  handle->run_state.shaft_angle = 0;
  while(Simple_Sensor_needsSearch(handle->sensor) && handle->run_state.shaft_angle < _2PI){
    angleOpenloop(handle,1.5f*_2PI);
    // call important for some sensors not to loose count
    // not needed for the search
   Simple_Sensor_update(handle->sensor);
  }
  // disable motor
  setPhaseVoltage(handle,0, 0, 0);
  // reinit the limits
  handle->motor_limit.velocity_limit = limit_vel;
  handle->motor_limit.voltage_limit = limit_volt;
  // check if the zero found
  //sensor->needsSearch();
  return !Simple_Sensor_needsSearch(handle->sensor);
}

// Iterative function looping FOC algorithm, setting Uq on the Motor
// The faster it can be run the better
void  simple_loopFOC(BLDCMotor_t *handle) 
{
  // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
  //                 of full rotations otherwise.
  if (handle->sensor) 
    Simple_Sensor_update(handle->sensor);

  // if open-loop do nothing
  if( handle->run_config.controller==MotionControl_angle_openloop || handle->run_config.controller==MotionControl_velocity_openloop )
    return;
  
  // if disabled do nothing
  if(!handle->enabled) 
    return;

  // Needs the update() to be called first
  // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
  // which is in range 0-2PI
  handle->run_state.electrical_angle = electricalAngle(handle);
  switch (handle->run_config.torque_controller) {
    case TorqueControl_voltage:
      // no need to do anything really
      break;
    case TorqueControl_dc_current:
      if(!handle->current_sense) return;
      // read overall current magnitude
       handle->run_state.current.q = Simple_getDCCurrent(handle->current_sense,handle->run_state.electrical_angle);
      // filter the value values
       handle->run_state.current.q = LowPassFilter_GetValue(&handle->LPF_current_q ,handle->run_state.current.q);
      // calculate the phase voltage
      handle->run_state.voltage.q = Simple_PIDController(&handle->PID_current_q,handle->run_state.current_sp - handle->run_state.current.q);
      // d voltage  - lag compensation
      if(_isset(handle->motor_physical.phase_inductance)) 
         handle->run_state.voltage.d = _constrain( -handle->run_state.current_sp* handle->run_state.shaft_velocity*handle->motor_physical.pole_pairs*handle->motor_physical.phase_inductance, -handle->motor_limit.voltage_limit, handle->motor_limit.voltage_limit);
      else  handle->run_state.voltage.d = 0;
      break;
    case TorqueControl_foc_current:
      if(!handle->current_sense) 
        return;
      // read dq currents
      handle->run_state.current = simple_getFOCCurrents(handle->current_sense,handle->run_state.electrical_angle);
      // filter values
      handle->run_state.current.q = LowPassFilter_GetValue(&handle->LPF_current_q,handle->run_state.current.q);
      handle->run_state.current.d = LowPassFilter_GetValue(&handle->LPF_current_d,handle->run_state.current.d);
      // calculate the phase voltages
      handle->run_state.voltage.q = LowPassFilter_GetValue(&handle->LPF_current_q,handle->run_state.current_sp - handle->run_state.current.q);
      handle->run_state.voltage.d = LowPassFilter_GetValue(&handle->LPF_current_d,-handle->run_state.current.d);
      // d voltage - lag compensation - TODO verify
      // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
      break;
    default:
      // no torque control selected
      //SIMPLEFOC_DEBUG("MOT: no torque control selected!");
      break;
  }

  // set the phase voltage - FOC heart function :)
  setPhaseVoltage(handle,handle->run_state.voltage.q, handle->run_state.voltage.d, handle->run_state.electrical_angle);
}

// Iterative function running outer loop of the FOC algorithm
// Behavior of this function is determined by the motor.controller variable
// It runs either angle, velocity or torque loop
// - needs to be called iteratively it is asynchronous function
// - if target is not set it uses motor.target value
void simple_move(BLDCMotor_t *handle,float new_target) 
{

  // downsampling (optional)
  //if(motion_cnt++ < motion_downsample) return;
  //motion_cnt = 0;

  // shaft angle/velocity need the update() to be called first
  // get shaft angle
  // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
  //                        For this reason it is NOT precise when the angles become large.
  //                        Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
  //                        when switching to a 2-component representation.
  if(handle->run_config.controller!=MotionControl_angle_openloop &&handle->run_config.controller!=MotionControl_velocity_openloop ) 
    handle->run_state.shaft_angle = shaftAngle(handle); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
  // get angular velocity  TODO the velocity reading probably also shouldn't happen in open loop modes?
  handle->run_state.shaft_velocity = shaftVelocity(handle); // read value even if motor is disabled to keep the monitoring updated

  // if disabled do nothing
  if(!handle->enabled) return;
  // set internal target variable
  if(_isset(new_target)) handle->run_state.target = new_target;
  
  // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
  if (_isset(handle->motor_physical.KV_rating)) handle->run_state.voltage_bemf = handle->run_state.shaft_velocity/handle->motor_physical.KV_rating/_RPM_TO_RADS;
  // estimate the motor current if phase reistance available and current_sense not available
  if(!handle->current_sense && _isset(handle->motor_physical.phase_resistance)) handle->run_state.current.q = (handle->run_state.voltage.q - handle->run_state.voltage_bemf)/handle->motor_physical.phase_resistance;

  // upgrade the current based voltage limit
  switch (handle->run_config.controller) {
    case MotionControl_torque:
      if(handle->run_config.torque_controller == TorqueControl_voltage){ // if voltage torque control
        if(!_isset(handle->motor_physical.phase_resistance))  
           handle->run_state.voltage.q = handle->run_state.target;
        else  handle->run_state.voltage.q =  handle->run_state.target*handle->motor_physical.phase_resistance+ handle->run_state.voltage_bemf;
        handle->run_state.voltage.q = _constrain(handle->run_state.voltage.q, -handle->motor_limit.voltage_limit, handle->motor_limit.voltage_limit);
        // set d-component (lag compensation if known inductance)
        if(!_isset( handle->motor_physical.phase_inductance)) 
           handle->run_state.voltage.d = 0;
        else 
           handle->run_state.voltage.d = _constrain( -handle->run_state.target*handle->run_state.shaft_velocity*handle->motor_physical.pole_pairs*handle->motor_physical.phase_inductance, -handle->motor_limit.voltage_limit, handle->motor_limit.voltage_limit);
      }else{
        handle->run_state.current_sp = handle->run_state.target; // if current/foc_current torque control
      }
      break;
    case MotionControl_angle:
      // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
      //                        the angles are large. This results in not being able to command small changes at high position values.
      //                        to solve this, the delta-angle has to be calculated in a numerically precise way.
      // angle set point
      handle->run_state.shaft_angle_sp = handle->run_state.target;
      // calculate velocity set point
      handle->run_state.shaft_velocity_sp = Simple_PIDController(&handle->P_angle, handle->run_state.shaft_angle_sp - handle->run_state.shaft_angle );
      // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
      handle->run_state.current_sp = Simple_PIDController(&handle->PID_velocity,handle->run_state.shaft_velocity_sp - handle->run_state.shaft_velocity); // if voltage torque control
      // if torque controlled through voltage
      if(handle->run_config.torque_controller == TorqueControl_voltage){
        // use voltage if phase-resistance not provided
        if(!_isset(handle->motor_physical.phase_resistance))  
           handle->run_state.voltage.q = handle->run_state.current_sp;
        else  
           handle->run_state.voltage.q =  _constrain( handle->run_state.current_sp*handle->motor_physical.phase_resistance + handle->run_state.voltage_bemf , -handle->motor_limit.voltage_limit,handle->motor_limit.voltage_limit);
        // set d-component (lag compensation if known inductance)
        if(!_isset(handle->motor_physical.phase_inductance)) 
           handle->run_state.voltage.d = 0;
        else 
           handle->run_state.voltage.d = _constrain( -handle->run_state.current_sp*handle->run_state.shaft_velocity*handle->motor_physical.pole_pairs*handle->motor_physical.phase_inductance, -handle->motor_limit.voltage_limit,handle->motor_limit.voltage_limit);
      }
      break;
    case MotionControl_velocity:
      // velocity set point - sensor precision: this calculation is numerically precise.
      handle->run_state.shaft_velocity_sp = handle->run_state.target;
      // calculate the torque command
      handle->run_state.current_sp = Simple_PIDController(&handle->PID_velocity,handle->run_state.shaft_velocity_sp - handle->run_state.shaft_velocity); // if current/foc_current torque control
      // if torque controlled through voltage control
      if(handle->run_config.torque_controller == TorqueControl_voltage){
        // use voltage if phase-resistance not provided
        if(!_isset(handle->motor_physical.phase_resistance))  
					//handle->run_state.current_sp=handle->run_state.current_sp;
           handle->run_state.voltage.q = handle->run_state.current_sp;
        else  
           handle->run_state.voltage.q = _constrain( handle->run_state.current_sp*handle->motor_physical.phase_resistance + handle->run_state.voltage_bemf , -handle->motor_limit.voltage_limit, handle->motor_limit.voltage_limit);
        // set d-component (lag compensation if known inductance)
        if(!_isset(handle->motor_physical.phase_inductance)) 
          handle->run_state.voltage.d = 0;
        else 
          handle->run_state.voltage.d = _constrain( -handle->run_state.current_sp*handle->run_state.shaft_velocity*handle->motor_physical.pole_pairs*handle->motor_physical.phase_inductance, -handle->motor_limit.voltage_limit, handle->motor_limit.voltage_limit);
      }
      break;
    case MotionControl_velocity_openloop:
      // velocity control in open loop - sensor precision: this calculation is numerically precise.
      handle->run_state.shaft_velocity_sp = handle->run_state.target;
      handle->run_state.voltage.q = velocityOpenloop(handle,handle->run_state.shaft_velocity_sp); // returns the voltage that is set to the motor
      handle->run_state.voltage.d = 0;
      break;
    case MotionControl_angle_openloop:
      // angle control in open loop - 
      // TODO sensor precision: this calculation NOT numerically precise, and subject
      //                        to the same problems in small set-point changes at high angles 
      //                        as the closed loop version.
      handle->run_state.shaft_angle_sp = handle->run_state.target;
      handle->run_state.voltage.q = angleOpenloop(handle,handle->run_state.shaft_angle_sp); // returns the voltage that is set to the motor
      handle->run_state.voltage.d = 0;
      break;
  }
}


// Method using FOC to set Uq and Ud to the motor at the optimal angle
// Function implementing Space Vector PWM and Sine PWM algorithms
//
// Function using sine approximation
// regular sin + cos ~300us    (no memory usage)
// approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
void setPhaseVoltage(BLDCMotor_t *handle, float Uq, float Ud, float angle_el)
{

  float center;
  int sector;
  float _ca,_sa;
	float Uout;

  switch (handle->run_config.foc_modulation)
  {
    case FOCModulation_Trapezoid_120 :
      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
      // determine the sector
      sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
      // centering the voltages around either
      // modulation_centered == true > driver.voltage_limit/2
      // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
      center = modulation_centered ? (handle->driver->voltage_limit)/2 : Uq;

      if(trap_120_map[sector][0]  == _HIGH_IMPEDANCE){
        handle->run_state.phase_pwm.Ua = center;
        handle->run_state.phase_pwm.Ub = trap_120_map[sector][1] * Uq + center;
        handle->run_state.phase_pwm.Uc = trap_120_map[sector][2] * Uq + center;
				BLDC_SetPhaseState(handle->driver,PHASE_OFF,PHASE_ON,PHASE_ON);// disable phase if possible
      }else if(trap_120_map[sector][1]  == _HIGH_IMPEDANCE){
        handle->run_state.phase_pwm.Ua = trap_120_map[sector][0] * Uq + center;
        handle->run_state.phase_pwm.Ub = center;
        handle->run_state.phase_pwm.Uc = trap_120_map[sector][2] * Uq + center;
				BLDC_SetPhaseState(handle->driver,PHASE_ON,PHASE_OFF,PHASE_ON);
      }else{
        handle->run_state.phase_pwm.Ua = trap_120_map[sector][0] * Uq + center;
        handle->run_state.phase_pwm.Ub = trap_120_map[sector][1] * Uq + center;
        handle->run_state.phase_pwm.Uc = center;
				BLDC_SetPhaseState(handle->driver,PHASE_ON,PHASE_ON,PHASE_OFF);// disable phase if possible
      }

    break;

    case FOCModulation_Trapezoid_150 :
      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
      // determine the sector
      sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
      // centering the voltages around either
      // modulation_centered == true > driver.voltage_limit/2
      // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
      center = modulation_centered ? (handle->driver->voltage_limit)/2 : Uq;

      if(trap_150_map[sector][0]  == _HIGH_IMPEDANCE){
        handle->run_state.phase_pwm.Ua= center;
        handle->run_state.phase_pwm.Ub = trap_150_map[sector][1] * Uq + center;
        handle->run_state.phase_pwm.Uc = trap_150_map[sector][2] * Uq + center;
				BLDC_SetPhaseState(handle->driver,PHASE_OFF,PHASE_ON,PHASE_ON); // disable phase if possible
      }else if(trap_150_map[sector][1]  == _HIGH_IMPEDANCE){
        handle->run_state.phase_pwm.Ua = trap_150_map[sector][0] * Uq + center;
        handle->run_state.phase_pwm.Ub = center;
        handle->run_state.phase_pwm.Uc = trap_150_map[sector][2] * Uq + center;
				BLDC_SetPhaseState(handle->driver,PHASE_ON,PHASE_OFF,PHASE_ON); // disable phase if possible
      }else if(trap_150_map[sector][2]  == _HIGH_IMPEDANCE){
        handle->run_state.phase_pwm.Ua = trap_150_map[sector][0] * Uq + center;
        handle->run_state.phase_pwm.Ub = trap_150_map[sector][1] * Uq + center;
        handle->run_state.phase_pwm.Uc = center;
				BLDC_SetPhaseState(handle->driver,PHASE_ON,PHASE_ON,PHASE_OFF); // disable phase if possible
      }else{
        handle->run_state.phase_pwm.Ua = trap_150_map[sector][0] * Uq + center;
        handle->run_state.phase_pwm.Ub = trap_150_map[sector][1] * Uq + center;
        handle->run_state.phase_pwm.Uc = trap_150_map[sector][2] * Uq + center;
				BLDC_SetPhaseState(handle->driver,PHASE_ON,PHASE_ON,PHASE_ON); // enable all phases
      }

    break;

    case FOCModulation_SinePWM :
      // Sinusoidal PWM modulation
      // Inverse Park + Clarke transformation

      // angle normalization in between 0 and 2pi
      // only necessary if using _sin and _cos - approximation functions
      angle_el = _normalizeAngle(angle_el);
      _ca = _cos(angle_el);
      _sa = _sin(angle_el);
      // Inverse park transform
      handle->run_state.phase_pwm.Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
      handle->run_state.phase_pwm.Ubeta =   _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;

      center =handle->driver->voltage_limit/2;
      // Clarke transform
      handle->run_state.phase_pwm.Ua =  handle->run_state.phase_pwm.Ualpha + center;
      handle->run_state.phase_pwm.Ub = -0.5f * handle->run_state.phase_pwm.Ualpha  + _SQRT3_2 * handle->run_state.phase_pwm.Ubeta + center;
      handle->run_state.phase_pwm.Uc = -0.5f * handle->run_state.phase_pwm.Ualpha - _SQRT3_2 *  handle->run_state.phase_pwm.Ubeta + center;

      if (!modulation_centered) {
        float Umin = _min(handle->run_state.phase_pwm.Ua, _min(handle->run_state.phase_pwm.Ub, handle->run_state.phase_pwm.Uc));
        handle->run_state.phase_pwm.Ua -= Umin;
        handle->run_state.phase_pwm.Ub -= Umin;
        handle->run_state.phase_pwm.Uc -= Umin;
      }

      break;

    case FOCModulation_SpaceVectorPWM :
      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
      // https://www.youtube.com/watch?v=QMSWUMEAejg

      // the algorithm goes
      // 1) Ualpha, Ubeta
      // 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
      // 3) angle_el = atan2(Ubeta, Ualpha)
      //
      // equivalent to 2)  because the magnitude does not change is:
      // Uout = sqrt(Ud^2 + Uq^2)
      // equivalent to 3) is
      // angle_el = angle_el + atan2(Uq,Ud)

      // a bit of optitmisation
      if(Ud){ // only if Ud and Uq set
        // _sqrt is an approx of sqrt (3-4% error)
        Uout = z_sqrt(Ud*Ud + Uq*Uq) /handle->driver->voltage_limit;
        // angle normalisation in between 0 and 2pi
        // only necessary if using _sin and _cos - approximation functions
        angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
      }else{// only Uq available - no need for atan2 and sqrt
        Uout = Uq / handle->driver->voltage_limit;
        // angle normalisation in between 0 and 2pi
        // only necessary if using _sin and _cos - approximation functions
        angle_el = _normalizeAngle(angle_el + _PI_2);
      }
      // find the sector we are in currently
      sector = floor(angle_el / _PI_3) + 1;
      // calculate the duty cycles
      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
      float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout;
      // two versions possible
      float T0 = 0; // pulled to 0 - better for low power supply voltage
      if (modulation_centered) {
        T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
      }

      // calculate the duty cycles(times)
      float Ta,Tb,Tc;
      switch(sector){
        case 1:
          Ta = T1 + T2 + T0/2;
          Tb = T2 + T0/2;
          Tc = T0/2;
          break;
        case 2:
          Ta = T1 +  T0/2;
          Tb = T1 + T2 + T0/2;
          Tc = T0/2;
          break;
        case 3:
          Ta = T0/2;
          Tb = T1 + T2 + T0/2;
          Tc = T2 + T0/2;
          break;
        case 4:
          Ta = T0/2;
          Tb = T1+ T0/2;
          Tc = T1 + T2 + T0/2;
          break;
        case 5:
          Ta = T2 + T0/2;
          Tb = T0/2;
          Tc = T1 + T2 + T0/2;
          break;
        case 6:
          Ta = T1 + T2 + T0/2;
          Tb = T0/2;
          Tc = T1 + T0/2;
          break;
        default:
         // possible error state
          Ta = 0;
          Tb = 0;
          Tc = 0;
      }

      // calculate the phase voltages and center
      handle->run_state.phase_pwm.Ua = Ta*handle->driver->voltage_limit;
      handle->run_state.phase_pwm.Ub = Tb*handle->driver->voltage_limit;
      handle->run_state.phase_pwm.Uc = Tc*handle->driver->voltage_limit;
      break;

  }
	BLDC_SetPWM(handle->driver,handle->run_state.phase_pwm.Ua, handle->run_state.phase_pwm.Ub, handle->run_state.phase_pwm.Uc);
}



// Function (iterative) generating open loop movement for target velocity
// - target_velocity - rad/s
// it uses voltage_limit variable
float velocityOpenloop(BLDCMotor_t *handle,float target_velocity)
{
  // get current timestamp
  unsigned long now_us = Simple_micros();
  // calculate the sample time from last call
  float Ts = (now_us - handle->open_loop_timestamp) * 1e-6f;
  // quick fix for strange cases (micros overflow + timestamp not defined)
  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;

  // calculate the necessary angle to achieve target velocity
  handle->run_state.shaft_angle = _normalizeAngle(handle->run_state.shaft_angle + target_velocity*Ts);
  // for display purposes
   handle->run_state.shaft_velocity = target_velocity;

  // use voltage limit or current limit
  float Uq = handle->motor_limit.voltage_limit;
  if(_isset(handle->motor_physical.phase_resistance)){
    Uq = _constrain(handle->motor_limit.current_limit*handle->motor_physical.phase_resistance + fabs(handle->run_state.voltage_bemf),-handle->motor_limit.voltage_limit, handle->motor_limit.voltage_limit);
    // recalculate the current  
    handle->run_state.current.q = (Uq - fabs(handle->run_state.voltage_bemf))/handle->motor_physical.phase_resistance;
  }
  // set the maximal allowed voltage (voltage_limit) with the necessary angle
  setPhaseVoltage(handle,Uq,  0, _electricalAngle(handle->run_state.shaft_angle, handle->motor_physical.pole_pairs));

  // save timestamp for next call
  handle->open_loop_timestamp = now_us;

  return Uq;
}

// Function (iterative) generating open loop movement towards the target angle
// - target_angle - rad
// it uses voltage_limit and velocity_limit variables
float angleOpenloop(BLDCMotor_t *handle,float target_angle)
{
  // get current timestamp
  unsigned long now_us = Simple_micros();
  // calculate the sample time from last call
  float Ts = (now_us - handle->open_loop_timestamp) * 1e-6f;
  // quick fix for strange cases (micros overflow + timestamp not defined)
  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;

  // calculate the necessary angle to move from current position towards target angle
  // with maximal velocity (velocity_limit)
  // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point
  //                        where small position changes are no longer captured by the precision of floats
  //                        when the total position is large.
  if(fabs( target_angle - handle->run_state.shaft_angle ) > fabs(handle->motor_limit.velocity_limit*Ts)){
    handle->run_state.shaft_angle += _sign(target_angle - handle->run_state.shaft_angle) * fabs(handle->motor_limit.velocity_limit )*Ts;
    handle->run_state.shaft_velocity = handle->motor_limit.velocity_limit;
  }else{
    handle->run_state.shaft_angle = target_angle;
    handle->run_state.shaft_velocity = 0;
  }

  // use voltage limit or current limit
  float Uq = handle->motor_limit.voltage_limit;
  if(_isset(handle->motor_physical.phase_resistance)){
    Uq = _constrain(handle->motor_limit.current_limit*handle->motor_physical.phase_resistance + fabs(handle->run_state.voltage_bemf),-handle->motor_limit.voltage_limit, handle->motor_limit.voltage_limit);
    // recalculate the current  
    handle->run_state.current.q = (Uq - fabs(handle->run_state.voltage_bemf))/handle->motor_physical.phase_resistance;
  }
  // set the maximal allowed voltage (voltage_limit) with the necessary angle
  // sensor precision: this calculation is OK due to the normalisation
  setPhaseVoltage(handle,Uq,  0, _electricalAngle(_normalizeAngle(handle->run_state.shaft_angle), handle->motor_physical.pole_pairs));

  // save timestamp for next call
  handle->open_loop_timestamp = now_us;

  return Uq;
}
